{
  "cells": [
    {
      "cell_type": "markdown",
      "id": "b67acc43",
      "metadata": {},
      "source": [
        "# ExtendedPriorFactor\n",
        "\n",
        "## Purpose and Audience\n",
        "\n",
        "`ExtendedPriorFactor` is the generalized building block that underlies {doc}`PriorFactor <PriorFactor.ipynb>` and other soft anchoring mechanisms. It lets you express an (optionally shifted) Gaussian (or robust) likelihood in the tangent space of an arbitrary manifold value type. This notebook targets advanced GTSAM users designing custom factors, experimenting with non-zero tangent-space means, or working with non-traditional manifold types."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "ffb7d481",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/nonlinear/doc/ExtendedPriorFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "id": "e3590b64",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "# Install GTSAM and Plotly from pip if running in Google Colab\n",
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop \n",
        "except ImportError:\n",
        "    pass # Not in Colab"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "d72ef00f",
      "metadata": {},
      "source": [
        "## Mathematical Definition\n",
        "\n",
        "Given a value type `T` (Lie group or general manifold) with local coordinates operator `Local(x, y)` (mapping from manifold to tangent space at `x`), an origin value `o`, an optional tangent-space mean vector `m` (default 0), and a noise model with (robust) loss `rho`, the factor defines:\n",
        "\n",
        "- Raw error:  `e(x) = - Local(x, o) - m`  (if `m` provided, else `-Local(x,o)`)\n",
        "- Loss: `loss(x) = rho( || e(x) ||^2_Σ )` where `Σ` is the covariance inside the noise model.\n",
        "- Likelihood: `L(x) = exp( - loss(x) )`.\n",
        "\n",
        "For Lie groups where `Local(x,o) = Log( x^{-1} o )`, this is (up to an additive constant in the exponent) the extended concentrated Gaussian:  \n",
        "$$  e(x) = - (\\operatorname{Log}(x^{-1} o) + m) $$\n",
        "\n",
        "Choosing a non-zero mean `m` effectively shifts the maximum-likelihood point from `o` to `o.retract(m)`."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "85c759eb",
      "metadata": {},
      "source": [
        "## Relation to PriorFactor\n",
        "\n",
        "`PriorFactor<T>` is a specialization that sets `m = 0` and names the origin `prior`. It also implements the Jacobian as identity by using the `-Local(x, prior)` form, which avoids computing derivatives of `Local`.\n",
        "\n",
        "Use `PriorFactor` unless you explicitly need a shifted tangent-space mean or want to treat the resulting distribution as a likelihood (e.g., for probabilistic weighting or annealing strategies)."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "fe71c221",
      "metadata": {},
      "source": [
        "## When to Use ExtendedPriorFactor Directly\n",
        "\n",
        "- You need a soft constraint centered at `origin.retract(mean)` but want to keep a fixed linearization frame at `origin` for numerical reasons.\n",
        "- You are implementing a factor that conceptually models a latent density over a manifold variable (e.g., pose priors from learned models producing mean offset vectors).\n",
        "- You want to evaluate normalized or relative likelihoods on candidate values (`likelihood(x)` convenience method).\n",
        "- You work with a robust noise model to downweight outliers in the tangent residual of a pseudo-prior.\n",
        "- You prototype new manifold types: avoiding providing analytic Jacobians for local coordinates beyond identity can simplify early experiments."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "8d1d7305",
      "metadata": {},
      "source": [
        "## C++ API Recap\n",
        "\n",
        "Constructor (templated on `VALUE`):\n",
        "\n",
        "```cpp\n",
        "ExtendedPriorFactor(Key key, const VALUE& origin,\n",
        "                    const SharedNoiseModel& model,\n",
        "                    const std::optional<Vector>& mean = {});\n",
        "```\n",
        "\n",
        "Key methods:\n",
        "\n",
        "- `Vector evaluateError(const VALUE& x, OptionalMatrixType H)` returns the tangent residual (with identity Jacobian if `H`).\n",
        "- `double error(const VALUE& x)` negative log-likelihood (robust).\n",
        "- `double likelihood(const VALUE& x)` returns `exp(-error(x))`.\n",
        "- Accessors: `origin()`, `mean()`."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "24207604",
      "metadata": {},
      "source": [
        "## Practical Example (Pose2)\n",
        "\n",
        "We replicate the core logic of the unit tests (see `nonlinear/tests/testExtendedPriorFactor.cpp`) using Python bindings."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "id": "fd83ff63",
      "metadata": {},
      "outputs": [
        {
          "data": {
            "text/plain": [
              "(array([ 0.00987086, -0.00524786,  0.        ]), 0.9997500833133725)"
            ]
          },
          "execution_count": 2,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "# Example: Pose2 ExtendedPriorFactor usage\n",
        "import math\n",
        "import numpy as np\n",
        "import gtsam\n",
        "from gtsam import Pose2, noiseModel\n",
        "\n",
        "key = 1\n",
        "origin = Pose2(1.0, 2.0, 0.3)\n",
        "model = noiseModel.Isotropic.Sigma(3, 0.5)  # std dev 0.5 in all dims\n",
        "mean = np.array([0.1, 0.2, 0.05])  # tangent space shift\n",
        "\n",
        "factor = gtsam.ExtendedPriorFactorPose2(key, origin, mean, model)\n",
        "# Evaluate at origin.retract(mean): should yield near-zero residual and likelihood ~ 1\n",
        "x_mode = origin.retract(mean)\n",
        "residual_at_mode = factor.evaluateError(x_mode)\n",
        "likelihood_at_mode = factor.likelihood(x_mode)\n",
        "residual_at_mode, likelihood_at_mode"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "7ca6bf92",
      "metadata": {},
      "source": [
        "### Interpreting the Residual\n",
        "\n",
        "Because the Jacobian reported is the identity, this factor is numerically stable and cheap: linearization contributes just an identity term. The residual is the signed difference in local coordinates, shifted by the mean."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "d0df0425",
      "metadata": {},
      "source": [
        "## Adding to a Graph\n",
        "\n",
        "You can add `ExtendedPriorFactor` directly to a `NonlinearFactorGraph`, but unlike `PriorFactor`, there is no sugar `addPrior<T>()`. You construct and add explicitly."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "id": "bd707769",
      "metadata": {},
      "outputs": [
        {
          "data": {
            "text/plain": [
              "(1.02536, 2.22216, 0.35)"
            ]
          },
          "execution_count": 3,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "from gtsam import NonlinearFactorGraph, Values, LevenbergMarquardtOptimizer\n",
        "\n",
        "graph = NonlinearFactorGraph()\n",
        "graph.push_back(factor)  # our ExtendedPriorFactor\n",
        "\n",
        "initial = Values()\n",
        "# Provide an initial guess somewhat away from the mode\n",
        "initial.insert(key, Pose2(1.2, 1.9, 0.25))\n",
        "\n",
        "params = gtsam.LevenbergMarquardtParams()\n",
        "optimizer = LevenbergMarquardtOptimizer(graph, initial, params)\n",
        "result = optimizer.optimize()\n",
        "final_pose = result.atPose2(key)\n",
        "final_pose"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "c14c53e5",
      "metadata": {},
      "source": [
        "## Design Notes and Pitfalls\n",
        "\n",
        "1. The frame of the tangent space is fixed at `origin`. A very large mean might implicitly request motion far from the linearization region; consider re-centering or using a different modeling approach if optimization struggles.\n",
        "2. Robust models (e.g. `noiseModel.Robust.Create(mEstimator, baseModel)`) apply their loss to the squared Mahalanobis distance of the residual; this lets you treat the tangent residual as an outlier-prone quantity.\n",
        "3. For custom manifold types, ensure `traits<T>::Local` and `retract` (implicitly via `retract` if you want to compute the *mode* value) are implemented and consistent.\n",
        "4. Unlike some priors, this factor purposefully does not store or recompute a dynamic Jacobian; the identity assumption is a deliberate design choice.\n",
        "5. Serialization support relies on `NoiseModelFactor1` for backward compatibility; keep that in mind if extending the class."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "b4ceb7e1",
      "metadata": {},
      "source": [
        "## When Not to Use\n",
        "\n",
        "- If you simply need a zero-mean soft prior: prefer {doc}`PriorFactor <PriorFactor.ipynb>`.\n",
        "- If you require a hard equality constraint: consider `NonlinearEquality`.\n",
        "- If the residual should depend on another variable or measurement, design a custom factor instead of embedding complexity in `Local`."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "9e49138f",
      "metadata": {},
      "source": [
        "## See Also\n",
        "\n",
        "- {doc}`PriorFactor <PriorFactor.ipynb>`\n",
        "- Other manifold priors: `BetweenFactor`, `NonlinearEquality`, and robust variants\n",
        "- Underlying noise models: `noiseModel::Isotropic`, `noiseModel::Diagonal`, robust creators"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}